/********************************************************************************
 * Copyright(c) 2020-2025 BST All rights reserved.
 * - Filename
 * - Author
 * - Version V1.0.0
 * - Date    2024/08/28
 * - Brief
 * - FunctionList:
 ******************************************************************************
 * History:
 *
 *
 *
 ******************************************************************************
 */

#include "InnoLidarAdapter.h"

#include <sys/time.h>

#include "TimeUtil.h"
#include "inno_lidar_log.h"
#include "inno_lidar_other_api.h"
#include "inno_lidar_packet_utils.h"

using namespace innolidar;
using namespace lidar;
InnoLidarAdapter::InnoLidarAdapter(const uint32_t &id, const std::string &ip,
                                   const uint16_t &msop_port,
                                   const uint16_t &difop_port)
    : _id(id), _ip(ip), _msopPort(msop_port), _difopPort(difop_port) {
  std::string info;
  info.append("Id:");
  info.append(std::to_string(_id));
  info.append(", IP:");
  info.append(_ip);
  info.append(", MsopPort:");
  info.append(std::to_string(_msopPort));
  info.append(", DifoPort:");
  info.append(std::to_string(_difopPort));
  std::cout << "Lidar Impl create : " << info << std::endl;

  inno_lidar_set_log_level(INNO_LOG_LEVEL_INFO);
  _innoLidarHandl = inno_lidar_open_live(
      "live", _ip.c_str(), _msopPort, INNO_LIDAR_PROTOCOL_PCS_UDP, _difopPort);
  if (_innoLidarHandl < 0) return;
  /* enable pointcloud output*/
  inno_lidar_set_attribute_string(_innoLidarHandl, "force_xyz_pointcloud", "1");

  inno_lidar_set_callbacks(
      _innoLidarHandl, NULL,
      [](const int h, void *ctx, const InnoDataPacket *pkt) -> int {
        return (reinterpret_cast<InnoLidarAdapter *>(ctx)->innoLidarPointCloudCB(h, *pkt));
      },
      NULL, NULL, this);
}

InnoLidarAdapter::~InnoLidarAdapter() {
  std::cout << "InnoLidarAdapter Destroy..." << std::endl;
}

void InnoLidarAdapter::start(
    std::function<void(std::shared_ptr<lpc::LidarProtoPointCloud> &&)>
        callback) {
  _isExit = false;
  int mLidarStatus;
  do {
    if (_isExit) {
      inno_lidar_stop(_innoLidarHandl);
      inno_lidar_close(_innoLidarHandl);
      return;
    }
    std::cout << "connect lidar  ..." << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    mLidarStatus = inno_lidar_start(_innoLidarHandl);

  } while (mLidarStatus != 0);

  _oneFrame = std::make_shared<lpc::LidarProtoPointCloud>();
  _packageThread.reset(new std::thread(
      std::bind(&InnoLidarAdapter::processLoop, this, callback)));
}

void InnoLidarAdapter::stop() {
  _isExit = true;
  inno_lidar_stop(_innoLidarHandl);
  inno_lidar_close(_innoLidarHandl);
  _condition.notify_all();
  if (_packageThread != nullptr && _packageThread->joinable()) {
    _packageThread->join();
  }
  std::cout << "LiarImpl Exit ..." << std::endl;
}

void InnoLidarAdapter::processLoop(
    const std::function<void(std::shared_ptr<lpc::LidarProtoPointCloud> &&)>
        &callback) {
  while (!_isExit) {
    std::unique_lock<std::mutex> lck(_mtx);
    _condition.wait(
        lck, [this] { return _isExit || !_lidarProtoPointCloudDeque.empty(); });
    if (_isExit) break;
    auto lidarFrame = _lidarProtoPointCloudDeque.front();
    _lidarProtoPointCloudDeque.pop_front();
    lck.unlock();
    if (_isExit) break;

    if (TimeUtil::fps_calc_inc(_fps)) {
      std::cout << "[FPS]: lidar " << lidarFrame->timestamp()
                << ", point count: " << lidarFrame->width()
                << " total fps:" << _fps.avg_fps << ", fps: " << _fps.ins_fps
                << std::endl;
    }
    callback(std::move(lidarFrame));
  }
}

int InnoLidarAdapter::innoLidarPointCloudCB(int handl,
                                            const InnoDataPacket &pkt) {
  for (int i = 0; i < pkt.item_number; i++) {
    if (pkt.type == INNO_ITEM_TYPE_XYZ_POINTCLOUD) {
      InnoXyzPoint *point =
          reinterpret_cast<InnoXyzPoint *>(const_cast<char *>(pkt.payload));
      if (_isNewFrame != (int64_t)pkt.idx) {
        /* add point counter */
        _oneFrame->set_width(_pointCounter);
        /* add last frame */
        {
          std::unique_lock<std::mutex> lck(_mtx);
          _lidarProtoPointCloudDeque.emplace_back(std::move(_oneFrame));
        }
        _condition.notify_one();
        _isNewFrame = pkt.idx;
        /*new frame comming*/
        _oneFrame = std::make_shared<lpc::LidarProtoPointCloud>();
        _oneFrame->set_lidar_id(_id);
        _oneFrame->set_system_timestamp(TimeUtil::now_ms());
        _oneFrame->set_timestamp((uint64_t)pkt.common.ts_start_us / 1000);
        _oneFrame->set_height(1);
        _oneFrame->set_is_dense(false);
        _pointCounter = 0;
      }
      /* point info */
      _oneFrame->add_data(point[i].x);
      _oneFrame->add_data(point[i].y);
      _oneFrame->add_data(point[i].z);
      _oneFrame->add_data(point[i].refl);
      _pointCounter++;
    }
  }

  return 0;
}